package ia.programasPrueba;

import lejos.nxt.ColorSensor;
import lejos.nxt.SensorPort;
import lejos.nxt.ColorSensor.Color;
import lejos.robotics.navigation.DifferentialPilot;
import ia.auxiliar.Arbitrator;
import ia.auxiliar.Behavior;
import ia.util.Connector;
import ia.util.CreadorPiloto;

public class EjemploComportamiento {
	
	private static ColorSensor cs;

	public static void main (String[] aArg){
		//Conectar, si no se puede terminar.
		boolean res = Connector.connectBT();
		if(!res) return;
		
		//Se crea el piloto
		final DifferentialPilot pilot = CreadorPiloto.getPilot();
		
		pilot.setRotateSpeed(40);
		pilot.setTravelSpeed(pilot.getMaxTravelSpeed()*0.3);	
		
		//Se crea el sensor
		cs = new ColorSensor(SensorPort.S1);
		cs.setFloodlight(Color.WHITE);
		
		Behavior compRojo = new Behavior()
		{
			public boolean takeControl() {
				if (cs.getColorID() == Color.RED)
					return true;
				return false;
				
			}
			public void suppress() {
				
			}
			public void action() {
				pilot.travel(20.0);
			}
		};
		
		Behavior compAzul = new Behavior()
		{
			public boolean takeControl() {
				if (cs.getColorID() == Color.BLUE)
					return true;
				return false;
				
			}
			public void suppress() {
				
			}
			public void action() {
				pilot.travel(10.0);
				pilot.rotate(-90);
				pilot.travel(10.0);
			}
		};
		
		Behavior compAmarillo = new Behavior()
		{
			public boolean takeControl() {
				if (cs.getColorID() == Color.YELLOW)
					return true;
				return false;
				
			}
			public void suppress() {
				
			}
			public void action() {
				pilot.travel(10.0);
				pilot.rotate(90);
				pilot.travel(10.0);
			}
		};
		
		Behavior compBlanco = new Behavior()
		{
			public boolean takeControl() {
				if (cs.getColorID() == Color.WHITE)
					return true;
				return false;
				
			}
			public void suppress() {
				
			}
			public void action() {
				pilot.stop();
			}
		};
		
		Behavior[] bArray = {compRojo, compAzul, compAmarillo};
		(new Arbitrator(bArray)).start();
	}
}
